/*	AX-12.c & .h
 *	This module is a revised interface for the Bioloid Ax-12 servos.  It aims to be easier to use than the previous
 *	version which is in dynamixel-comms.c & .h
 *
 *	Author:  Phil Hall
 *	Date:    May 2008
 *
*/
#ifndef AX-12_h
#define AX-12_h

#define AX12_BROADCAST_ID			254
#define AX12_MAX_POSITION			1023
#define AX12_MIN_POSITION			0
#define AX12_MAX_SPEED				1023
#define AX12_MIN_SPEED				1

#define AX12_MAX_PACKET_LEN			257

#define AX12_TIMEOUT				30000L

// AX12 Memory addresses

//--- Control Table Address ---
//EEPROM AREA
#define AX12_MODEL_NUMBER_L      	0
#define AX12_MODEL_NUMBER_H      	1
#define AX12_VERSION             	2
#define AX12_ID                  	3
#define AX12_BAUD_RATE           	4
#define AX12_RETURN_DELAY_TIME   	5
#define AX12_CW_ANGLE_LIMIT_L    	6
#define AX12_CW_ANGLE_LIMIT_H    	7
#define AX12_CCW_ANGLE_LIMIT_L   	8
#define AX12_CCW_ANGLE_LIMIT_H   	9
#define AX12_SYSTEM_DATA2        	10
#define AX12_LIMIT_TEMPERATURE   	11
#define AX12_DOWN_LIMIT_VOLTAGE  	12
#define AX12_UAX12_LIMIT_VOLTAGE    13
#define AX12_MAX_TORQUE_L        	14
#define AX12_MAX_TORQUE_H        	15
#define AX12_RETURN_LEVEL        	16
#define AX12_ALARM_LED           	17
#define AX12_ALARM_SHUTDOWN      	18
#define AX12_OPERATING_MODE      	19
#define AX12_DOWN_CALIBRATION_L  	20
#define AX12_DOWN_CALIBRATION_H  	21
#define AX12_UAX12_CALIBRATION_L    22
#define AX12_UAX12_CALIBRATION_H    23

#define AX12_TORQUE_ENABLE          24
#define AX12_LED                   (25)
#define AX12_CW_COMPLIANCE_MARGIN  (26)
#define AX12_CCW_COMPLIANCE_MARGIN (27)
#define AX12_CW_COMPLIANCE_SLOPE   (28)
#define AX12_CCW_COMPLIANCE_SLOPE  (29)
#define AX12_GOAL_POSITION_L       (30)
#define AX12_GOAL_POSITION_H       (31)
#define AX12_GOAL_SPEED_L          (32)
#define AX12_GOAL_SPEED_H          (33)
#define AX12_TORQUE_LIMIT_L        (34)
#define AX12_TORQUE_LIMIT_H        (35)
#define AX12_PRESENT_POSITION_L    (36)
#define AX12_PRESENT_POSITION_H    (37)
#define AX12_PRESENT_SPEED_L       (38)
#define AX12_PRESENT_SPEED_H       (39)
#define AX12_PRESENT_LOAD_L        (40)
#define AX12_PRESENT_LOAD_H        (41)
#define AX12_PRESENT_VOLTAGE       (42)
#define AX12_PRESENT_TEMPERATURE   (43)
#define AX12_REGISTERED_INSTRUCTION (44)
#define AX12_PAUSE_TIME            (45)
#define AX12_MOVING				   (46)
#define AX12_LOCK                  (47)
#define AX12_PUNCH_L               (48)
#define AX12_PUNCH_H               (49)

// AX-12 Commands
#define AX12_PING           		0x01
#define AX12_READ           		0x02
#define AX12_WRITE          		0x03
#define AX12_REG_WRITE      		0x04
#define AX12_ACTION         		0x05
#define AX12_RESET          		0x06
#define AX12_DIGITAL_RESET  		0x07
#define AX12_SYSTEM_READ    		0x0C
#define AX12_SYSTEM_WRITE   		0x0D
#define AX12_SYNC_WRITE     		0x83
#define AX12_SYNC_REG_WRITE 		0x84

//	Function return values (i.e. error codes) for this module
//	Functions normally return the length of a status packet (i.e. a +ve number) or an error code

#define AX12_ERR_TIMEOUT			-1
#define AX12_ERR_BAD_PREAMBLE		-2
#define AX12_ERR_BAD_CHECKSUM		-3
#define AX12_ERR_BAD_PACKET_LEN		-4
#define AX12_ERR_BAD_SERVO_ID		-5
#define AX12_ERR_BUFF_TOO_SHORT		-6
#define AX12_ERR_OUT_OF_BOUNDS		-7
#define AX12_SERVO_ERROR			-8


#define RS485_TXD PORTE &= ~_BV(PE3),PORTE |= _BV(PE2)  //_485_DIRECTION = 1
#define RS485_RXD PORTE &= ~_BV(PE2),PORTE |= _BV(PE3)  //PORT_485_DIRECTION = 0
#define cbi(REG8,BITNUM) REG8 &= ~(_BV(BITNUM))
#define sbi(REG8,BITNUM) REG8 |= _BV(BITNUM)
#define SET_TxD0_FINISH   sbi(UCSR0A,6)
#define RESET_TXD0_FINISH cbi(UCSR0A,6)
#define CHECK_TXD0_FINISH bit_is_set(UCSR0A,6)
#define SET_TxD1_FINISH  sbi(UCSR1A,6)
#define RESET_TXD1_FINISH cbi(UCSR1A,6)
#define CHECK_TXD1_FINISH bit_is_set(UCSR1A,6)

extern unsigned char DebugServoID;

void AX12TxPacket(unsigned char *Data);
	//	Data must be a properly formed packet but not including the preamble or checksum

int AX12RxByte(unsigned char *Byte);

int AX12RxPacket(unsigned char *Data, int ExpectedLen, unsigned char ServoID, int BuffLen);
	//	The returned packet will not have the pre-amble or the checksum.
	//	Returns length of packet for a good read or an error code
	//	Checks the packet checksum.  Returns AX12_ERR_BAD_CHECKSUM if wrong.
	//	if ExpectedLen is non-zero then it is checked. Do not include preamble or checksum in this count.
	//	if ServoID is non-zero then it is checked.  Returns AX12_ERR_BAD_SERVO_ID if wrong.
	//	Returns AX12_ERR_BUFF_TOO_SHORT if the provided buffer is too short
	//	Does not check the status byte within the packet

int AX12CheckStatus(unsigned char *Packet);
	//	Checks the Status byte in the given packet

int AX12ReadMemory(unsigned char ServoID, unsigned char StartAddress, unsigned char BytesToRead, unsigned char *Data);

int AX12WriteMemory(unsigned char ServoID, unsigned char StartAddress, unsigned char BytesToWrite, unsigned char *Data);

int AX12WriteUShort(unsigned char ServoID, unsigned char StartAddress, unsigned short ushort);

int AX12ReadUShort(unsigned char ServoID, unsigned char StartAddress, unsigned short *ushort);
	//	Reads an unsigned short from StartAddress & StartAddress+1

int AX12GetPosition(unsigned char ServoID, unsigned short *Position);
	//	Gets the current position of the specified servo

int AX12SetPosition(unsigned char ServoID, unsigned short Position, unsigned short Speed);
	//	Sets the target position and speed of the specified servo
	
int AX12SetMode(unsigned char ServoID, int Mode);
		//	Mode != 1:	Servo seeks goal position
		//	Mode == 1:	Free rotating mode

int AX12SetFreeSpinSpeed(unsigned char ServoID, short Speed);
		//	Sets the speed when in free rotating mode (see AX12SetMode())
		//	Speed should be in the range -511 to +511. -ve implies anti-clockwise rotation

int AX12Ping(unsigned char ServoID);

int AX12SetBounds(unsigned short *Val, unsigned short MinVal, unsigned short MaxVal);
	//	Forces *Val to be within MinVal & MaxVal.  Returns true if was already within bounds

void AX12ReportPacket(unsigned char *Packet);

void CommsPrintf(char *Format, ...);


#endif